#include <stdlib.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <string.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <pthread.h>
#include <stdio.h>
#include <errno.h>
#ifdef GTK
#include <glib.h>
#endif

#include "netcom.h"
#include "packet.h"
#include "robot.h"

status_t connection_new_robot(conn_t **conn, char *address, uint16_t port)
{
	*conn = (conn_t *) malloc (sizeof (conn_t));
	(*conn)->data = (robot_data_t *) malloc (sizeof (robot_data_t));

	(*conn)->sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
	strcpy((*conn)->address, address);
	(*conn)->port = port;
	
	if ( -1 == (*conn)->sock ) {
		fprintf(stderr, "socket creation error.\n");
		return SOCKET_FAIL;
	}
	
	memset(&((*conn)->sa), 0, sizeof (*conn)->sa);
	(*conn)->sa.sin_family = AF_INET;
	(*conn)->sa.sin_addr.s_addr = inet_addr(address);
	(*conn)->sa.sin_port = htons(port);

    memset((*conn)->data, 0, sizeof(robot_data_t));
    
	return SUCCESS;
}

status_t connection_dispose(conn_t *conn)
{
	free(conn->data);
	close(conn->sock);

	return SUCCESS;
}

status_t connection_send_stream(conn_t *conn, uint8_t *stream, uint16_t len)
{
	uint16_t sentb = 0;

	if ( (NULL == conn) || (NULL == stream) ) {
		return NULL_POINTER;
	}

	sentb = sendto(conn->sock, stream, len, 0, 
		(struct sockaddr *) &conn->sa, sizeof conn->sa);

#ifdef DEBUG
	print_packet_stream(stderr, stream);
#endif

	if ( len != sentb ) {
		return SOCKET_FAIL;
	}

	return SUCCESS;
}

status_t connection_recv_stream(conn_t *conn, uint8_t *stream, uint16_t *len)
{
	uint16_t readb = 0;
	socklen_t fromlen = 0;

	readb = recvfrom(conn->sock, (void *) stream, READ_BUF_SIZE, 0,
		(struct sockaddr *) &conn->sa, &fromlen);
	*len = readb;

#ifdef DEGUG
	print_packet_stream(stderr, stream);
#endif

	return SUCCESS;
}

void *stream_read(void *arg)
{
	conn_t *conn = arg;
	uint8_t buf[READ_BUF_SIZE];
//	uint8_t pstream[MAX_PACKET_LEN];
	uint16_t offset = 0,  bread = -1;
	uint16_t streamno = 0;
	packet_t *packet;

#ifdef GTK
	fprintf(stderr, "glib thread\n");
#else
	pthread_detach(pthread_self());

	fprintf(stderr, "Thread ID:%u entering infinite read loop\n", 
			(unsigned)pthread_self());
#endif
	while( 0 != bread ) {
		connection_recv_stream(conn, buf, &bread);
#ifdef DEBUG
//		fprintf(stderr, "stream read: %d with bytecount: %d -->", 
//x						streamno++,
//						bread);
#endif
//		offset = find_packet_stream(buf, offset, bread, pstream);

		if ( buf[0] == 0x5e && buf[1] == 0x02 &&
			buf[bread-2] == 0x5e && buf[bread-1] == 0x0d &&
			SUCCESS == validate_packet_stream(buf)) {
#ifdef DEBUG
//x			fprintf(stderr, "packet found with no. %d; dlen=%d \n", 
//x					buf[INDEX_RSV], buf[INDEX_LEN]);
#endif
			packet_deserialize(&packet, buf);
			robot_data_set(packet, conn->data);
			packet_delete(packet);
			offset = 0;
		} else {
#ifdef DEBUG
			fprintf(stderr, "no packet found\n");
#endif
		}
		memset(buf, 0, READ_BUF_SIZE);
//		while ( offset < bread ) {
//			offset = find_packet_stream(buf, offset, 
//							bread, pstream);
//			fprintf(stderr, "offset:%d, bread:%d\n", 
//				offset, bread);
//			if ( offset < bread ) {
//				if ( SUCCESS == 
//					validate_packet_stream(pstream) ) {
//					print_packet_stream(stdout, pstream);
//					packet_deserialize(&packet, pstream);
//					robot_data_set(packet, conn->data);
//					packet_delete(packet);
//					sleep(1);
//				} else {
//					fprintf(stdout, "not valid\n");
//				}
//			} else {
//				offset = 0;
//				break;
//			}
//
//		}
	} 
#ifndef GTK
	pthread_exit(NULL);
#endif
	return NULL; 
    
}

status_t connection_get_data(conn_t *conn)
{

	fprintf(stderr, "creating thread for reading socket\n");
#ifdef GTK
	GThread *thread;
	thread = g_thread_create((GThreadFunc)stream_read, (void *)conn, 
							TRUE, NULL);
#endif
#ifdef IPAD

#else
	static pthread_t read_thread;		
  pthread_create(&read_thread, 0, stream_read, (void *)conn);
//	pthread_exit(NULL);
#endif
    fprintf(stderr, "exiting thread instantiating routine\n");
    
	return SUCCESS;
}
